/**
 * @file	commander_events.c
 * @author	Mitch, David
 * @brief	Controller for events coordinated scheduling policy.
 */
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdio.h>
#include <string.h>
#include <util/delay.h>
#include "header/commander.h"

#ifdef COMMANDER_EVENTS
/* flag set when receiving packet */
volatile uint8_t rxflag = 0;

/* flag set when user presses joystick command */
uint8_t joystick_flag = 0;

/* flag set when speed needs to be printed */
uint8_t uart_flag = 0;

/* buffer for holding speed information */
uint8_t output[64];

/* data structure holding distance, start time, and end time */
ROOMBA_DATA data;

/* packet to be sent containing Roomba command */
radiopacket_t command_packet;

/* event variable that is signaled or waited upon for sending packet */
EVENT *sendEvent;

/* event variable that is signaled or waited upon for receiving packet */
EVENT *receiveEvent;

/* event variable that is signaled or waited upon for printing */
EVENT *printEvent;

const unsigned char PPP[] = {};	// Execute the periodic task every 200 ticks (1 second)
const unsigned int PT = sizeof(PPP)/2;	// There are 4 tasks in the PPP array.

/* Station, Roomba and our address */
uint8_t station_addr[5] = { 0x98, 0x76, 0x54, 0x32, 0x10 };
uint8_t roomba_addr[5] = { 0xED, 0xB7, 0xED, 0xB7, 0xED };	//0xEDB7EDB7ED
uint8_t commander_addr[5] = { 0x12, 0x12, 0x12, 0x12, 0x12 };

/* Roomba commands */
uint8_t left_command[4] = {0x00, 0x64, 0x00, 0x01};
uint8_t right_command[4] = {0x00, 0x64, 0xFF, 0xFF};
uint8_t left_up_command[4] = {0x00, 0xC8, 0x01, 0xF4};	//left and up
uint8_t right_up_command[4] = {0x00, 0xC8, 0xFF, 0x04};	//right and up
uint8_t up_100_command[4] = {0x00, 0x64, 0x80, 0x00};	//forward 100mm/s
uint8_t up_200_command[4] = {0x00, 0xC8, 0x80, 0x00};	//200mm/s
uint8_t up_500_command[4] = {0x01, 0xF4, 0x80, 0x00};	//500mm/s
uint8_t down_100_command[4] = {0xFF, 0x9C, 0x80, 0x00};	//backward 100mm/s
uint8_t down_200_command[4] = {0xFF, 0x38, 0x80, 0x00};	//backward 200mm/s
uint8_t down_300_command[4] = {0xFE, 0xD4, 0x80, 0x00};	//backward 200mm/s
uint8_t select_command[4] = {0x00, 0x00, 0x00, 0x00};

/**
 * @brief 	Main function. Loops forever to poll I/Os.
 */
int main(void) {
	//initializing leds
	init_led();

	//initializing joystick
	init_joystick();

	//setting bit rate
	UART_BPS bitrate = UART_38400;

	//initializing uart
	uart_init(bitrate);

	cli();

	//initializing radio
	init_radio();

	sei();

	//initializing send event
	sendEvent = Event_Init();

	//initializing receive event
	receiveEvent = Event_Init();

	//initializing print event
	printEvent = Event_Init();

	//create joystick as round robin task
	Task_Create(task_joystick, 0, RR, 0);

	//create radio tx as system task
	Task_Create(task_radio_send, 1, SYSTEM, 1);

	//create radio rx as system task
	Task_Create(task_radio_receive, 2, SYSTEM, 2);

	//create uart as round robin task
	Task_Create(task_uart, 0, RR, 0);
	return 0;
}

/**
 * @brief 	Initialize LED port D for output
 */
void init_led() {
	//use pins 4,5,6 and 7 on Port D for output
	DDRD = (1<<PORTD4) | (1<<PORTD5) | (1<<PORTD6) | (1<<PORTD7);
}

/**
 * @brief 	Initialize joystick buttons
 */
void init_joystick() {
	//enable select button
	DDRB &= (1<<PB5);
	PORTB |= (1<<PB5);

	//enable left button
	DDRB &= (1<<PB6);
	PORTB |= (1<<PB6);

	//enable up button
	DDRB &= (1<<PB7);
	PORTB |= (1<<PB7);

	//enable right button
	DDRE &= (1<<PE4);
	PORTE |= (1<<PE4);

	//enable down button
	DDRE &= (1<<PE5);
	PORTE |= (1<<PE5);
}

/**
 * @brief 	Initialize Radio
 */
void init_radio() {
	Radio_Init();

	Radio_Configure_Rx(RADIO_PIPE_0, commander_addr, ENABLE);

	Radio_Configure(RADIO_2MBPS, RADIO_HIGHEST_POWER);

	Radio_Set_Tx_Addr(roomba_addr);
}

/**
 * @brief If any joystick direction is pressed,
 * this task will prepare a command packet,
 * and set joystick flag high,
 * and create a radio send system task,
 * and sleep for 40 ticks (200ms)
 */
void task_joystick(void) {
	for (;;) {
		//if left is pressed
		if (!bit_is_set(PINB, PB6)) {
			set_roomba_command(&command_packet, DRIVE, left_up_command);
			joystick_flag = 1;
			Event_Signal(sendEvent);
			//Task_Create(task_radio_send, 1, SYSTEM, 1);
			Task_Sleep(40);	//Put joystick to sleep for 200 ms
		}
		//if right is pressed
		else if (!bit_is_set(PINE, PE4)) {
			set_roomba_command(&command_packet, DRIVE, right_up_command);
			joystick_flag = 1;
			Event_Signal(sendEvent);
			//Task_Create(task_radio_send, 1, SYSTEM, 1);
			Task_Sleep(40);
		}
		//if select is pressed
		else if (!bit_is_set(PINB, PB5)) {
			set_roomba_command(&command_packet, DRIVE, select_command);
			joystick_flag = 1;
			Event_Signal(sendEvent);
			//Task_Create(task_radio_send, 1, SYSTEM, 1);
			Task_Sleep(40);
		}
		//if up is pressed
		else if (!bit_is_set(PINB, PB7)) {
			set_roomba_command(&command_packet, DRIVE, up_200_command);
			joystick_flag = 1;
			Event_Signal(sendEvent);
			//Task_Create(task_radio_send, 1, SYSTEM, 1);
			Task_Sleep(40);
		}
		//if down is pressed
		else if (!bit_is_set(PINE, PE5)) {
			set_roomba_command(&command_packet, DRIVE, down_100_command);
			joystick_flag = 1;
			Event_Signal(sendEvent);
			//Task_Create(task_radio_send, 1, SYSTEM, 1);
			Task_Sleep(40);
		}
	}
}

/**
 * @brief Send command to Roomba
 */
void task_radio_send(void) {
	for (;;) {

		if (joystick_flag) {
			joystick_flag = 0;
			Radio_Transmit(&command_packet, RADIO_WAIT_FOR_TX);
		}
		Event_Wait(sendEvent);
	}
}

/**
 * @brief Receive Roomba status command
 */
void task_radio_receive(void) {
	radiopacket_t status_packet;

	for (;;) {
		//statement entered upon receiving status packet from roomba
		if (rxflag) {
			//store previous time for later calculations
			data.start_time = data.end_time;

			//get time
			data.end_time = Now();

			_delay_ms(20);

			if (Radio_Receive(&status_packet) != RADIO_RX_MORE_PACKETS) {
				rxflag = 0;
			}

			//get distance (millimeters) from status packet
			data.distance = status_packet.payload.sensors.sensors.distance.value;

			uart_flag = 1;
			Event_Signal(printEvent);
			//Task_Create(task_uart, 2, SYSTEM, 2);	//Create uart as periodic task
		}
		Event_Wait(receiveEvent);
	}
}

/**
 * @brief Print characters to the screen through uart
 */
void task_uart(void) {
	int i;
	int16_t time = 0;
	float speed = 0;

	for (;;) {
		if (uart_flag) {

			//calculate time elapsed (milliseconds) since last packet received
			time = (data.end_time - data.start_time) * 5;

			//calculate speed and convert from mm/ms to mm/s
			speed = ((float) data.distance / time) * 1000;

			snprintf((char *)output, 64, " Distance %d, Time: %d (ms), Speed: %d (mm/s) \n\r",data.distance, time, (int) speed);

			for (i = 0; i < 64 && output[i] != 0; i++) {
				uart_putchar(output[i]);
			}
			uart_flag = 0;
		}
		Event_Wait(printEvent);
	}
}

/**
 * @brief Set Roomba command
 *
 * @param packet a packet to be sent to Roomba
 * @param command a command to be sent to Roomba
 * @param arguments arguments to be sent to Roomba, first 2 bytes: avg velocity (mm/s), second 2 bytes: radius (mm)
 *
 */
void set_roomba_command(radiopacket_t *packet, uint8_t command, uint8_t arguments[NUM_ARGS_BYTES]) {
	memcpy(command_packet.payload.command.sender_address, commander_addr, 5); //copy sender address to packet
	packet->type = COMMAND;
	packet->payload.command.command = command;
	packet->payload.command.arguments[0] = arguments[0];
	packet->payload.command.arguments[1] = arguments[1];
	packet->payload.command.arguments[2] = arguments[2];
	packet->payload.command.arguments[3] = arguments[3];
	packet->payload.command.num_arg_bytes = NUM_ARGS_BYTES;
}


void radio_rxhandler(uint8_t pipenumber)
{
	rxflag = 1;
	Event_Signal(receiveEvent);
}

#endif
